#include "Aria.h"
#include <stdio.h>
#include <iostream>
#include <string>
#include "RobotController.h"

using namespace std;

int main (int argc, char* argv[]){

//Inizializzo la libreria
Aria::init();

//Definisco un robot
ArRobot robot;

//Analisi degli argomenti e connessione
ArArgumentParser parser(&argc, argv);
ArSimpleConnector connector(&parser);

//Connessione con il robot
parser.loadDefaultArguments();
if(!connector.parseArgs()){
	cout<<"Errore: impostazioni sconoscite! Il programma verrà terminato!";
	Aria::exit(0);
	exit(1);
}if(!connector.connectRobot(&robot)){
	cout<<"Errore: impossibile avviare la connessione! Il programma verrà terminato!";
	Aria::exit(0);
	exit(1);
}

//Prendo il lock sul robot ed inserisco la configurazione
robot.lock();
//Avvio i motori
robot.comInt(ArCommands::ENABLE,1);
//Rilascio il robot
robot.unlock();

//Avvio il sonar
//ArSonarDevice sonar;
//robot.addRangeDevice(&sonar);

RobotController controller(&robot);
controller.run();

robot.run(true);
Aria::shutdown();
return 0;

}
